/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include "main.h"
#ifdef FLOATING_POINT
#include "showdata.h"
#endif
#include <stdlib.h>
#include <stdio.h>
//#include <math.h>
#include "lp_anal.h"
#include "pctolsf.h"
#include "bwexp.h"
#include "celp_sup.h"
#include "code_lsf.h"
#include "zero_array16.h"
#include "auto_cor.h"

STATIC void ApplyWindow(
fxpt_16 SpeechIn[F_LEN],
fxpt_16 HamSpeech[F_LEN]);

#if 0 /* function now defined in auto_cor.c */
STATIC void AutoCor(
fxpt_16 speech_in[F_LEN],
fxpt_32 ac[ORDER+1]);
#endif

STATIC void ACtoPC(
fxpt_32 ac[ORDER+1],
fxpt_32 pc[ORDER+1]);

#ifdef FLOATING_POINT
STATIC void ACtoRC(
float ac[ORDER+1],
float rc[ORDER],
float *err);

STATIC void HighFreqCorrect(
float   ac[ORDER+1],
float   alpha);

STATIC void RCtoPC(
float   k[ORDER],
float   pc[ORDER]);
#endif

STATIC void Durbin(
fxpt_32 ac[],
fxpt_32 pc[]);

/**************************************************************************
*                                                                         *
* ROUTINE
*               LP_Analysis
*
* FUNCTION
*               Linear Predictive analysis
* SYNOPSIS
*               LP_Analysis(SpeechIn, qlsf, frame_num)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       SpeechIn        fxpt_16  i      Frame of input speech
*       qlsf            fxpt_16  o      Frame of quantized LSFs
*       frame_num       int      i      CELP frame number
*
**************************************************************************
*
* DESCRIPTION
*
*       This performs an autocorrelation LP analysis of speech
*       returning a set of line spectral frequencies representing
*       the spectrum of the input speech signal.
*
**************************************************************************
*
* CALLED BY
*
*       Analysis
*
* CALLS
*
*       ApplyWindow AutoCor ACtoPC BWexp_PCs PCtoLSF QuanLSF
*
*
**************************************************************************/

#define         TRUE    1
#define         FALSE   0

void LP_Analysis(
fxpt_16 SpeechIn[F_LEN],                /* 15.0 format */
fxpt_16 qlsf[ORDER]                     /* 0.15 format */
)
{
        fxpt_32 ac[ORDER+1];                    /* 30.1 format */
        fxpt_16 lsf[ORDER+1];                   /* 0.15 format */
        fxpt_32 newpc[ORDER+1];					/* 5.16 format */
//        fxpt_16 pc[ORDER+1];					/* 2.13 format */
		fxpt_16 pcexp[ORDER+1];					/* 2.13 format */
        fxpt_16 HamSpeech[F_LEN];               /* 15.0 format */
        int     findex[ORDER];

//		DUMPARR0(SpeechIn,F_LEN)

FXPT_PUSHSTATE("LP_Analysis", -1.0, -1.0);
        /* Apply Hamming window to Input Speech */
        ApplyWindow(SpeechIn, HamSpeech);

		DUMPARR0(HamSpeech,F_LEN)

        /* Calculate Autocorrelations */
        AutoCor(HamSpeech, ac);

		DUMPARR(ac,1,ORDER+1)

        /* Convert ACs to PCs */
        ACtoPC(ac, newpc);

		DUMPARR(newpc,26,ORDER+1)

        /* Bandwidth Expand PCs */
        BWExpand32(OMEGA32, newpc, pcexp);

		DUMPARR(pcexp,13,ORDER+1)

        /* Convert PCs to LSFs */
        PCtoLSF(pcexp, lsf);

		DUMPARR(lsf,15,ORDER+1)

        /* Quantize the LSFs */
        QuanLSF(lsf, qlsf, findex);

		DUMPARR(qlsf,15,ORDER)
		DUMPARR0(findex,ORDER);

FXPT_POPSTATE();
}


/**************************************************************************
*                                                                         *
* ROUTINE
*               ApplyWindow
*
* FUNCTION
*               Apply a Hamming Window to input speech
* SYNOPSIS
*               ApplyWindow(SpeechIn, HamSpeech)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       SpeechIn        fxpt_16   i     Frame of input speech
*       HamSpeech       fxpt_16   o     Frame of output speech
*
**************************************************************************/

void ApplyWindow(
fxpt_16 SpeechIn[F_LEN],                /* 15.0 format */
fxpt_16 HamSpeech[F_LEN])               /* 15.0 format */
{
        static int      first=TRUE;
        static fxpt_16  HamWindow[F_LEN];       /* 0.15 format */
        int             i;

        /* Generate Hamming window */
        if (first) {
                first=FALSE;
                CreateHam(HamWindow, F_LEN);
        }


        /* Apply Hamming window to input speech */
        for (i=0;i<F_LEN;i++) {
                /*HamSpeech[i] = SpeechIn[i] * HamWindow[i];*/
                HamSpeech[i] = fxpt_mult16_round(SpeechIn[i], HamWindow[i], 15);
        }

}


/**************************************************************************
*                                                                         *
* ROUTINE
*               AutoCor
*
* FUNCTION
*               Calculate the autocorrelations for a frame of speech
* SYNOPSIS
*               AutoCor(speech_in, ac)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       speech_in       fxpt_16   i     Frame of input speech
*       ac              fxpt_32   o     Autocorrelations
*
**************************************************************************/
#if 0  /* function now defined in auto_cor.c */
void AutoCor(
fxpt_16 speech_in[F_LEN],               /* 15.0 format */
fxpt_32 ac[ORDER+1])                    /* 30.1 format */
{
        int i, j;

        /*  Calculate autocorrelations */
        for (i=0; i<ORDER+1; i++) {
                ac[i] = 0;
                for (j=i; j<F_LEN; j++)
                        /*ac[i]+=speech_in[j]*speech_in[j-i];*/
                        /* The right shifts are for normalization.
                         * F_LEN is 60, so we want to multiply each
                         * value by 1/60 before accumulating it.  The
                         * right shifts approximate this by effectively
                         * multiplying each value by 1/64.
                         */
#if 0
                        ac[i] = fxpt_mac32(ac[i],
                            fxpt_shr16_round(speech_in[j], 3),
                            fxpt_shr16_round(speech_in[j-i], 3));
#else
                        ac[i] = fxpt_add32(ac[i], fxpt_shr32_fast(
                            fxpt_mult32(speech_in[j], speech_in[j-i]), 6));
#endif
        }
}
#endif


/**************************************************************************
*                                                                         *
* ROUTINE
*               ACtoPC
*
* FUNCTION
*               Calculate the Predictor Coefficients for a frame of speech
* SYNOPSIS
*               ACtoPC(ac, pc)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       ac              fxpt_32  i      Autocorrelations
*       pc              fxpt_16  o      Predictor Coefficients
*
**************************************************************************/

void ACtoPC(
fxpt_32 ac[ORDER+1],
fxpt_32 pc[ORDER+1])
{
#ifdef DURBIN
        /*  Use Durbin recursion to convert from ACs to PCs */
        pc[0] = 67108864L;           /* 1.0 in 5.16 format */
        ZeroArray32(&pc[1], ORDER);
        Durbin(ac, pc);
#else
        float   alpha;
        float   rc[ORDER];

        /* Convert ACs to RCs */
        ACtoRC(ac, rc, &alpha);

        /*  Perform high frequency correction */
        HighFreqCorrect(ac, alpha);

        /*  Convert corrected ACs to RCs */
        ACtoRC(ac, rc, &alpha);

        /*  Convert corrected RCs to PCs */
        RCtoPC(rc, pc);
#endif
}

/*****************************************************************************
*
* NAME
*       Durbin
*
*
* SYNOPSIS
*
*       subroutine Durbin (ac, pc)
*
*   formal
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       ac[ORDER]       fxpt_32 i       auto-correlation coefficients
*       pc[ORDER+1]     fxpt_16 o       prediction coeffs (voiced-> +pc[1])
*
****************************************************************************
*
* DESCRIPTION
*
*       This performs the classical Durbin recursion
*       on the correlation sequence c[0], c[1], c[2] . . .
*       to obtain n reflection coefficients (rc).
*
*       The sign convention used defines the first reflection coefficient
*       as the normalized first autocorrelation coefficient, which results
*       in positive values of rc[0] for voiced speech.
*
****************************************************************************
*
* CALLED BY
*
*       ACtoPC
*
* CALLS
*
*       ZeroArray16
*
*
***************************************************************************
*
* REFERENCES
*
*       Parsons, Voice and Speech Processing, McGraw-Hill, 1987, p.160&378.
*
****************************************************************************/

void Durbin(
fxpt_32 ac[],                   /* 30.1 format */
fxpt_32 pc[])                   /* 5.16 format */
{
        int i, j;
        fxpt_32 alpha, beta;
//        fxpt_16 rc[ORDER], tmp[ORDER];
        fxpt_32 rc[ORDER], tmp[ORDER];      /* 5.26 format */
        fxpt_32 newpc[ORDER];				/* 5.26 format */

        /* If zero energy, set rc's to zero & return  */
		/* WARNING: bigmac says: should be setting pc???, not rc!!! */
        if (ac[0] <= 0) {
                for (i = 0; i < ORDER; i++)
                        rc[i] = 0;
                return;
        }

        /* Intialization   */

        alpha = ac[0];
        /*pc[0] = rc[0] = -ac[1] / ac[0];*/
        newpc[0] = rc[0] = fxpt_div32(fxpt_negate32(ac[1]), ac[0], 26);
        beta = ac[1];

        /* Recursion   */

        for (i = 1; i < ORDER; i++) {
                /*alpha = alpha + beta * rc[i - 1];*/
                alpha = fxpt_add32(alpha, fxpt_mult64_round_good(beta,
                    rc[i-1], 26));
                beta = ac[i+1];

                for (j = 0; j <= i - 1; j++)
                        /*beta = beta + ac[j+1] * pc[i-j-1];*/
                        beta = fxpt_add32(beta, fxpt_mult64_round_good(ac[j+1],
                            newpc[i-j-1], 26));

                /*rc[i] = -beta / alpha;*/
                rc[i] = fxpt_div32(fxpt_negate32(beta), alpha, 26);

                for (j = 0; j <= i - 1; j++)
                        /*tmp[j] = rc[i] * pc[i-j-1];*/
                        tmp[j] = fxpt_mult64_round_good(rc[i], newpc[i-j-1], 26);

                for (j = 0; j <= i - 1; j++)
                        /*pc[j] = pc[j] + tmp[j];*/
                        newpc[j] = fxpt_add32(newpc[j], tmp[j]);

                newpc[i] = rc[i];
        }

		DUMPARR(newpc,26,ORDER)

        /* Rearrange array ordering and set pc[0]=1 */

        for (i = ORDER; i > 0; i--)
//                pc[i] = fxpt_extract_h( fxpt_shl32_fast( newpc[i-1], 3 ) );
                pc[i] = newpc[i-1];
//                pc[i] = pc[i-1];
        pc[0] = 67108864L;           /* 1.0 in 5.26 format */
}


/***************************************************************************
*
* NAME
*       ACtoRC
*
* FUNCTION
*
*       Schur recursion to do autocorrelation analysis.
*       Converts autocorrelation sequence to reflection coefficients.
*
* SYNOPSIS
*
*       subroutine ACtoRC (ac, rc, err)
*
*   formal
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       ac(ORDER+1)     float   i       auto-correlation coefficients
*       rc(ORDER)       float   o       reflection coefficients (voiced->+rc1)
*       err             float   i/o     normalized prediction error
*
***************************************************************************
*
* DESCRIPTION
*
*       This performs the classical Schur recursion (rediscovered by
*       LeRoux & Guegen) on the correlation sequence ac(0), ac(1), ac(2)...
*       to obtain n reflection coefficients (rc).  The recursion can be
*       performed entirely in fractional arithemetic because the
*       correlation sequence is normalized by ac(0), so all the quantities
*       in the recursion are less than unity magnitude (except ac(0)).
*
*       The sign convention used defines the first reflection coefficient
*       as the normalized first autocorrelation coefficient, which results
*       in positive values of rc(1) for voiced speech.
*
***************************************************************************
*
* CALLED BY
*
*       ACtoPC
*
* CALLS
*
*       ZeroArray16
*
***************************************************************************
*
* REFERENCES
*
*       Parsons, Voice and Speech Processing, McGraw-hill, 1987, p.160&378.
*
***************************************************************************
*
* PROCESS DESCRIPTION
*
*       Name    Type    Function
*
*       d       float      Recursion sequence (dim n)
*       g       float      Recursion sequence (dim n)
*       p       int        Orderindex recursion
*       err     float      Backward error
*       err0    float      Backward error of order 0
*
**************************************************************************/

#ifdef FLOATING_POINT
void ACtoRC(
float ac[ORDER+1],
float rc[ORDER],
float *err)
{
  float *d, *g, rch;
  int i, p;

  d=(float *)calloc(ORDER, sizeof(float));
  if(d == NULL) {
    printf("Error callocating memory (d) in actorc\n");
    exit(1);
  }

  g=(float *)calloc(ORDER, sizeof(float));
  if(g == NULL) {
    printf("Error callocating memory (g) in actorc\n");
    exit(1);
  }

/* If zero energy, set rc's to zero & return    */
  if (ac[0] <= 0.0)   {
    ZeroArray16(rc, ORDER);
    return;
  }
  for (i = 0; i < ORDER; i++)
    g[i] = d[i] = ac[i+1] / ac[0];
  rch = g[0];
  rc[0] = rch;
  *err = 1. - rch * rch;
  for (p = 1; p < ORDER; p++)
  {
    for (i = 0; i < ORDER - p; i++)
    {
      g[i] = g[i+1] - rch * d[i];
      d[i] = d[i] - rch * g[i+1];
    }

/* Extract the reflection coefficient   */
    rch = g[0] / *err;
    rc[p] = rch;

/* The mean squares error recursion */
    *err = *err * (1. - rch * rch);
  }
  free(d);
  free(g);
}
#endif

/**************************************************************************
*                                                                         *
* ROUTINE
*               HighFreqCorrect
*
* FUNCTION
*               Perform high frequency correction on input
*               (eq. 16, Atal & Schroeder)
* SYNOPSIS
*               HighFreqCorrect(ac, alpha)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       ac              float     i     Frame of autocorrelation coefficients
*       alpha           float     i     Normalized prediction error
*
**************************************************************************/
#ifdef FLOATING_POINT
void HighFreqCorrect(
float   ac[ORDER+1],
float   alpha)
{
float   lemin;

/*  Unnormalized prediction error scaled by lambda */
        lemin = LAMBDA * ac[0] * alpha;

/* High Frequency Correction  */
        ac[0]  += 0.375  * lemin;
        ac[1] -= 0.25   * lemin;
        ac[2] += 0.0625 * lemin;
}
#endif

/***************************************************************************
*
* NAME
*       RCtoPC
*
* FUNCTION
*
*       convert reflection coefficients into lpc coefficients
*
*       BEWARE:  This code does not use memory efficiently.
*
* SYNOPSIS
*
*       subroutine RCtoPC(k, pc)
*
*   formal
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       k               float   i       reflection coefficients (m)
*       pc              float   o       predictor parameters (m+1: a(1)=1.0)
*
***************************************************************************
*
* DESCRIPTION
*
*       Converts reflection coefficients into lpc coefficients.
*
*       CELP's LP predictor coefficient convention is:
*              p+1         -(i-1)
*       A(z) = SUM   a   z          where a  = +1.0
*              i=1    i                    1
*
*
***************************************************************************
*
* CALLED BY
*
*       ACtoPC
*
***************************************************************************
*
* REFERENCES
*
*       Rabiner & Schafer, Digital Processing of Speech Signals,
*       Prentice-Hall, 1978, p. 443, equations 8.131a&b&c.
*
**************************************************************************/
#ifdef FLOATING_POINT
void RCtoPC(
float   k[ORDER],
float   pc[ORDER])
{
  int i, j;
  float a[ORDER+1][ORDER+1];

/* intialize the array                                           */
  for (i = 0; i <= ORDER; i++)
  {
    for (j = 0; j <= ORDER; j++)
    {
      a[j][i] = 0.0;
    }
  }

/* convert reflection coefficients                              */
  for (i = 1; i <= ORDER; i++)
  {
    a[i][i] = k[i-1];
    for (j = 1; j <= i-1; j++)
      a[j][i] = a[j][i-1] - k[i-1] * a[i-j][i-1];
  }
  pc[0] = 1.0;
  for (j = 1; j <= ORDER; j++)
    pc[j] = -a[j][ORDER];
}
#endif
